$OpenBSD: patch-rttable_c,v 1.2 2016/12/23 13:44:31 rzalamena Exp $
--- rttable.c.orig	Thu Dec 15 19:50:24 2016
+++ rttable.c	Mon Dec 19 21:22:11 2016
@@ -38,15 +38,22 @@
 */
 
 #include "defs.h"
+#include <sys/queue.h>
+#include <sys/tree.h>
     
 /**
 *   Routing table structure definition. Double linked list...
 */
+struct Origin {
+    TAILQ_ENTRY(Origin) next;
+    uint32		originAddr;
+    int			flood;
+    uint32		pktcnt;
+};
+
 struct RouteTable {
-    struct RouteTable   *nextroute;     // Pointer to the next group in line.
-    struct RouteTable   *prevroute;     // Pointer to the previous group in line.
+    RB_ENTRY(RouteTable) entry;
     uint32              group;          // The group to route
-    uint32              originAddr;     // The origin adress (only set on activated routes)
     uint32              vifBits;        // Bits representing recieving VIFs.
 
     // Keeps the upstream membership state...
@@ -56,28 +63,58 @@
     uint32              ageVifBits;     // Bits representing aging VIFs.
     int                 ageValue;       // Downcounter for death.          
     int                 ageActivity;    // Records any acitivity that notes there are still listeners.
+    TAILQ_HEAD(originhead, Origin) originList; // The origin adresses (non-empty on activated routes)
 };
+RB_HEAD(rtabletree, RouteTable) routing_table =
+    RB_INITIALIZER(&routing_table);
 
-                 
-// Keeper for the routing table...
-static struct RouteTable   *routing_table;
-
 // Prototypes
 void logRouteTable(char *header);
 int  internAgeRoute(struct RouteTable*  croute);
+int  internUpdateKernelRoute(struct RouteTable *route, int activate, struct Origin *o);
 
-// Socket for sending join or leave requests.
-int mcGroupSock = 0;
+int			 rtable_cmp(struct RouteTable *, struct RouteTable *);
+struct RouteTable	*rtable_add(struct RouteTable *);
+void			 rtable_remove(struct RouteTable *);
+RB_GENERATE(rtabletree, RouteTable, entry, rtable_cmp);
 
+int
+rtable_cmp(struct RouteTable *rt, struct RouteTable *rtn)
+{
+	if (rt->group < rtn->group)
+		return (-1);
 
+	return (rt->group > rtn->group);
+}
+
+struct RouteTable *
+rtable_add(struct RouteTable *rt)
+{
+	return (RB_INSERT(rtabletree, &routing_table, rt));
+}
+
+void
+rtable_remove(struct RouteTable *rt)
+{
+	struct Origin		*o;
+
+        while ((o = TAILQ_FIRST(&rt->originList))) {
+		TAILQ_REMOVE(&rt->originList, o, next);
+		free(o);
+        }
+
+	RB_REMOVE(rtabletree, &routing_table, rt);
+	free(rt);
+}
+
 /**
 *   Function for retrieving the Multicast Group socket.
 */
 int getMcGroupSock() {
-    if( ! mcGroupSock ) {
-        mcGroupSock = openUdpSocket( INADDR_ANY, 0 );;
+    if (MRouterFD < 0) {
+        log(LOG_ERR, errno, "no MRouterFD.");
     }
-    return mcGroupSock;
+    return MRouterFD;
 }
  
 /**
@@ -87,11 +124,8 @@
     unsigned Ix;
     struct IfDesc *Dp;
 
-    // Clear routing table...
-    routing_table = NULL;
-
     // Join the all routers group on downstream vifs...
-    for ( Ix = 0; Dp = getIfByIx( Ix ); Ix++ ) {
+    for ( Ix = 0; (Dp = getIfByIx( Ix )); Ix++ ) {
         // If this is a downstream vif, we should join the All routers group...
         if( Dp->InAdr.s_addr && ! (Dp->Flags & IFF_LOOPBACK) && Dp->state == IF_STATE_DOWNSTREAM) {
             IF_DEBUG log(LOG_DEBUG, 0, "Joining all-routers group %s on vif %s",
@@ -159,29 +193,24 @@
 *   Clear all routes from routing table, and alerts Leaves upstream.
 */
 void clearAllRoutes() {
-    struct RouteTable   *croute, *remainroute;
+    struct RouteTable   *croute;
 
     // Loop through all routes...
-    for(croute = routing_table; croute; croute = remainroute) {
-
-        remainroute = croute->nextroute;
-
+    while ((croute = RB_ROOT(&routing_table)) != NULL) {
         // Log the cleanup in debugmode...
         IF_DEBUG log(LOG_DEBUG, 0, "Removing route entry for %s",
                      inetFmt(croute->group, s1));
 
         // Uninstall current route
-        if(!internUpdateKernelRoute(croute, 0)) {
+        if(!internUpdateKernelRoute(croute, 0, NULL)) {
             log(LOG_WARNING, 0, "The removal from Kernel failed.");
         }
 
         // Send Leave message upstream.
         sendJoinLeaveUpstream(croute, 0);
 
-        // Clear memory, and set pointer to next route...
-        free(croute);
+	rtable_remove(croute);
     }
-    routing_table = NULL;
 
     // Send a notice that the routing table is empty...
     log(LOG_NOTICE, 0, "All routes removed. Routing table is empty.");
@@ -192,15 +221,10 @@
 *   Route Descriptor.
 */
 struct RouteTable *findRoute(uint32 group) {
-    struct RouteTable*  croute;
+	struct RouteTable	 key;
 
-    for(croute = routing_table; croute; croute = croute->nextroute) {
-        if(croute->group == group) {
-            return croute;
-        }
-    }
-
-    return NULL;
+	key.group = group;
+	return (RB_FIND(rtabletree, &routing_table, &key));
 }
 
 /**
@@ -212,7 +236,6 @@
     
     struct Config *conf = getCommonConfig();
     struct RouteTable*  croute;
-    int result = 1;
 
     // Sanitycheck the group adress...
     if( ! IN_MULTICAST( ntohl(group) )) {
@@ -241,9 +264,7 @@
         newroute = (struct RouteTable*)malloc(sizeof(struct RouteTable));
         // Insert the route desc and clear all pointers...
         newroute->group      = group;
-        newroute->originAddr = 0;
-        newroute->nextroute  = NULL;
-        newroute->prevroute  = NULL;
+        TAILQ_INIT(&newroute->originList);
 
         // The group is not joined initially.
         newroute->upstrState = ROUTESTATE_NOTJOINED;
@@ -260,54 +281,13 @@
             BIT_SET(newroute->vifBits, ifx);
         }
 
-        // Check if there is a table already....
-        if(routing_table == NULL) {
-            // No location set, so insert in on the table top.
-            routing_table = newroute;
-            IF_DEBUG log(LOG_DEBUG, 0, "No routes in table. Insert at beginning.");
-        } else {
+        if ((croute = rtable_add(newroute)) != NULL)
+            free(newroute);
+	else {
+            // Set the new route as the current...
+            croute = newroute;
+	}
 
-            IF_DEBUG log(LOG_DEBUG, 0, "Found existing routes. Find insert location.");
-
-            // Check if the route could be inserted at the beginning...
-            if(routing_table->group > group) {
-                IF_DEBUG log(LOG_DEBUG, 0, "Inserting at beginning, before route %s",inetFmt(routing_table->group,s1));
-
-                // Insert at beginning...
-                newroute->nextroute = routing_table;
-                newroute->prevroute = NULL;
-                routing_table = newroute;
-
-                // If the route has a next node, the previous pointer must be updated.
-                if(newroute->nextroute != NULL) {
-                    newroute->nextroute->prevroute = newroute;
-                }
-
-            } else {
-
-                // Find the location which is closest to the route.
-                for( croute = routing_table; croute->nextroute != NULL; croute = croute->nextroute ) {
-                    // Find insert position.
-                    if(croute->nextroute->group > group) {
-                        break;
-                    }
-                }
-
-                IF_DEBUG log(LOG_DEBUG, 0, "Inserting after route %s",inetFmt(croute->group,s1));
-                
-                // Insert after current...
-                newroute->nextroute = croute->nextroute;
-                newroute->prevroute = croute;
-                if(croute->nextroute != NULL) {
-                    croute->nextroute->prevroute = newroute; 
-                }
-                croute->nextroute = newroute;
-            }
-        }
-
-        // Set the new route as the current...
-        croute = newroute;
-
         // Log the cleanup in debugmode...
         log(LOG_INFO, 0, "Inserted route table entry for %s on VIF #%d",
             inetFmt(croute->group, s1),ifx);
@@ -325,10 +305,10 @@
             inetFmt(croute->group, s1), ifx);
 
         // If the route is active, it must be reloaded into the Kernel..
-        if(croute->originAddr != 0) {
+        if(!TAILQ_EMPTY(&croute->originList)) {
 
             // Update route in kernel...
-            if(!internUpdateKernelRoute(croute, 1)) {
+            if(!internUpdateKernelRoute(croute, 1, NULL)) {
                 log(LOG_WARNING, 0, "The insertion into Kernel failed.");
                 return 0;
             }
@@ -351,7 +331,7 @@
 *   activated, it's reinstalled in the kernel. If
 *   the route is activated, no originAddr is needed.
 */
-int activateRoute(uint32 group, uint32 originAddr) {
+int activateRoute(uint32 group, uint32 originAddr, int downIf) {
     struct RouteTable*  croute;
     int result = 0;
 
@@ -369,21 +349,42 @@
     }
 
     if(croute != NULL) {
+	struct Origin *o = NULL;
+	int found = 0;
+
         // If the origin address is set, update the route data.
-        if(originAddr > 0) {
-            if(croute->originAddr > 0 && croute->originAddr!=originAddr) {
-                log(LOG_WARNING, 0, "The origin for route %s changed from %s to %s",
-                    inetFmt(croute->group, s1),
-                    inetFmt(croute->originAddr, s2),
-                    inetFmt(originAddr, s3));
-            }
-            croute->originAddr = originAddr;
-        }
+	if(originAddr > 0) {
 
-        // Only update kernel table if there are listeners !
-        if(croute->vifBits > 0) {
-            result = internUpdateKernelRoute(croute, 1);
-        }
+	    TAILQ_FOREACH(o, &croute->originList, next) {
+		log(LOG_INFO, 0, "Origin for route %s have %s, new %s",
+		    inetFmt(croute->group, s1),
+		    inetFmt(o->originAddr, s2),
+		    inetFmt(originAddr, s3));
+		if (o->originAddr==originAddr) {
+		    found++;
+		    break;
+		}
+	    }
+	    if (!found) {
+		log(LOG_NOTICE, 0, "New origin for route %s is %s, flood %d",
+		    inetFmt(croute->group, s1),
+		    inetFmt(originAddr, s3), downIf);
+		o = malloc(sizeof(*o));
+		o->originAddr = originAddr;
+		o->flood = downIf;
+		o->pktcnt = 0;
+		TAILQ_INSERT_TAIL(&croute->originList, o, next);
+	    } else {
+		log(LOG_INFO, 0, "Have origin for route %s at %s, pktcnt %d",
+		    inetFmt(croute->group, s1),
+		    inetFmt(o->originAddr, s3),
+		    o->pktcnt);
+	    }
+	}
+
+        // Only update kernel table if there are listeners, but flood upstream!
+        if(croute->vifBits > 0 || downIf >= 0)
+            result = internUpdateKernelRoute(croute, 1, o);
     }
     IF_DEBUG logRouteTable("Activate Route");
 
@@ -401,11 +402,7 @@
     IF_DEBUG log(LOG_DEBUG, 0, "Aging routes in table.");
 
     // Scan all routes...
-    for( croute = routing_table; croute != NULL; croute = nroute ) {
-        
-        // Keep the next route (since current route may be removed)...
-        nroute = croute->nextroute;
-
+    RB_FOREACH_SAFE(croute, rtabletree, &routing_table, nroute) {
         // Run the aging round algorithm.
         if(croute->upstrState != ROUTESTATE_CHECK_LAST_MEMBER) {
             // Only age routes if Last member probe is not active...
@@ -443,7 +440,6 @@
 *   route is not found, or not in this state, 0 is returned.
 */
 int lastMemberGroupAge(uint32 group) {
-    struct Config       *conf = getCommonConfig();
     struct RouteTable   *croute;
 
     croute = findRoute(group);
@@ -477,7 +473,7 @@
     //BIT_ZERO(croute->vifBits);
 
     // Uninstall current route from kernel
-    if(!internUpdateKernelRoute(croute, 0)) {
+    if(!internUpdateKernelRoute(croute, 0, NULL)) {
         log(LOG_WARNING, 0, "The removal from Kernel failed.");
         result = 0;
     }
@@ -489,24 +485,8 @@
         sendJoinLeaveUpstream(croute, 0);
     }
 
-    // Update pointers...
-    if(croute->prevroute == NULL) {
-        // Topmost node...
-        if(croute->nextroute != NULL) {
-            croute->nextroute->prevroute = NULL;
-        }
-        routing_table = croute->nextroute;
+    rtable_remove(croute);
 
-    } else {
-        croute->prevroute->nextroute = croute->nextroute;
-        if(croute->nextroute != NULL) {
-            croute->nextroute->prevroute = croute->prevroute;
-        }
-    }
-    // Free the memory, and set the route to NULL...
-    free(croute);
-    croute = NULL;
-
     IF_DEBUG logRouteTable("Remove route");
 
     return result;
@@ -551,6 +531,36 @@
         }
     }
 
+    {
+	struct Origin *o, *nxt;
+	struct sioc_sg_req sg_req;
+
+	sg_req.grp.s_addr = croute->group;
+	for (o = TAILQ_FIRST(&croute->originList); o; o = nxt) {
+	    nxt = TAILQ_NEXT(o, next);
+	    sg_req.src.s_addr = o->originAddr;
+	    if (ioctl(MRouterFD, SIOCGETSGCNT, (char *)&sg_req) < 0) {
+		log(LOG_WARNING, errno, "%s (%s %s)",
+		    "age_table_entry: SIOCGETSGCNT failing for",
+		    inetFmt(o->originAddr, s1),
+		    inetFmt(croute->group, s2));
+		/* Make sure it gets deleted below */
+		sg_req.pktcnt = o->pktcnt;
+	    }
+	    log(LOG_DEBUG, 0, "Aging Origin %s Dst %s PktCnt %d -> %d",
+		inetFmt(o->originAddr, s1), inetFmt(croute->group, s2),
+		o->pktcnt, sg_req.pktcnt);
+	    if (sg_req.pktcnt == o->pktcnt) {
+		/* no traffic, remove from kernel cache */
+		internUpdateKernelRoute(croute, 0, o);
+		TAILQ_REMOVE(&croute->originList, o, next);
+		free(o);
+	    } else {
+		o->pktcnt = sg_req.pktcnt;
+	    }
+	}
+    }
+
     // If the aging counter has reached zero, its time for updating...
     if(croute->ageValue == 0) {
         // Check for activity in the aging process,
@@ -560,7 +570,7 @@
                          inetFmt(croute->group,s1));
             
             // Just update the routing settings in kernel...
-            internUpdateKernelRoute(croute, 1);
+            internUpdateKernelRoute(croute, 1, NULL);
     
             // We append the activity counter to the age, and continue...
             croute->ageValue = croute->ageActivity;
@@ -586,34 +596,58 @@
 /**
 *   Updates the Kernel routing table. If activate is 1, the route
 *   is (re-)activated. If activate is false, the route is removed.
+*   if 'origin' is given, only the route with 'origin' will be
+*   updated, otherwise all MFC routes for the group will updated.
 */
-int internUpdateKernelRoute(struct RouteTable *route, int activate) {
+int internUpdateKernelRoute(struct RouteTable *route, int activate, struct Origin *origin) {
     struct   MRouteDesc     mrDesc;
     struct   IfDesc         *Dp;
     unsigned                Ix;
+    struct Origin *o;
     
-    if(route->originAddr>0) {
+    if (TAILQ_EMPTY(&route->originList)) {
+        log(LOG_NOTICE, 0, "Route is not active. No kernel updates done.");
+        return 1;
+    }
+    TAILQ_FOREACH(o, &route->originList, next) {
+	if (origin && origin != o)
+		continue;
 
         // Build route descriptor from table entry...
         // Set the source address and group address...
         mrDesc.McAdr.s_addr     = route->group;
-        mrDesc.OriginAdr.s_addr = route->originAddr;
+        mrDesc.OriginAdr.s_addr = o->originAddr;
     
         // clear output interfaces 
         memset( mrDesc.TtlVc, 0, sizeof( mrDesc.TtlVc ) );
     
-        IF_DEBUG log(LOG_DEBUG, 0, "Vif bits : 0x%08x", route->vifBits);
+        IF_DEBUG log(LOG_DEBUG, 0, "Origin %s Vif bits : 0x%08x", inetFmt(o->originAddr, s1), route->vifBits);
 
         // Set the TTL's for the route descriptor...
-        for ( Ix = 0; Dp = getIfByIx( Ix ); Ix++ ) {
-            if(Dp->state == IF_STATE_UPSTREAM) {
-                //IF_DEBUG log(LOG_DEBUG, 0, "Identified VIF #%d as upstream.", Dp->index);
-                mrDesc.InVif = Dp->index;
-            }
-            else if(BIT_TST(route->vifBits, Dp->index)) {
-                IF_DEBUG log(LOG_DEBUG, 0, "Setting TTL for Vif %d to %d", Dp->index, Dp->threshold);
-                mrDesc.TtlVc[ Dp->index ] = Dp->threshold;
-            }
+        for ( Ix = 0; (Dp = getIfByIx( Ix )); Ix++ ) {
+	    if (o->flood >= 0) {
+		if(Ix == o->flood) {
+		    IF_DEBUG log(LOG_DEBUG, 0, "Identified Input VIF #%d as DOWNSTREAM.", Dp->index);
+		    mrDesc.InVif = Dp->index;
+		}
+		else if(Dp->state == IF_STATE_UPSTREAM) {
+		    IF_DEBUG log(LOG_DEBUG, 0, "Setting TTL for UPSTREAM Vif %d to %d", Dp->index, Dp->threshold);
+		    mrDesc.TtlVc[ Dp->index ] = Dp->threshold;
+		}
+		else if(BIT_TST(route->vifBits, Dp->index)) {
+		    IF_DEBUG log(LOG_DEBUG, 0, "Setting TTL for DOWNSTREAM Vif %d to %d", Dp->index, Dp->threshold);
+		    mrDesc.TtlVc[ Dp->index ] = Dp->threshold;
+		}
+	    } else {
+		if(Dp->state == IF_STATE_UPSTREAM) {
+		    IF_DEBUG log(LOG_DEBUG, 0, "Identified VIF #%d as upstream.", Dp->index);
+		    mrDesc.InVif = Dp->index;
+		}
+		else if(BIT_TST(route->vifBits, Dp->index)) {
+		    IF_DEBUG log(LOG_DEBUG, 0, "Setting TTL for Vif %d to %d", Dp->index, Dp->threshold);
+		    mrDesc.TtlVc[ Dp->index ] = Dp->threshold;
+		}
+	    }
         }
     
         // Do the actual Kernel route update...
@@ -625,9 +659,6 @@
             // Delete the route from Kernel...
             delMRoute( &mrDesc );
         }
-
-    } else {
-        log(LOG_NOTICE, 0, "Route is not active. No kernel updates done.");
     }
 
     return 1;
@@ -639,29 +670,27 @@
 */
 void logRouteTable(char *header) {
     IF_DEBUG  {
-        struct RouteTable*  croute = routing_table;
+        struct RouteTable*  croute = RB_ROOT(&routing_table);
         unsigned            rcount = 0;
     
         log(LOG_DEBUG, 0, "\nCurrent routing table (%s);\n-----------------------------------------------------\n", header);
         if(croute==NULL) {
             log(LOG_DEBUG, 0, "No routes in table...");
         } else {
-            do {
-                /*
-                log(LOG_DEBUG, 0, "#%d: Src: %s, Dst: %s, Age:%d, St: %s, Prev: 0x%08x, T: 0x%08x, Next: 0x%08x",
-                    rcount, inetFmt(croute->originAddr, s1), inetFmt(croute->group, s2),
-                    croute->ageValue,(croute->originAddr>0?"A":"I"),
-                    croute->prevroute, croute, croute->nextroute);
-                */
-                log(LOG_DEBUG, 0, "#%d: Src: %s, Dst: %s, Age:%d, St: %s, OutVifs: 0x%08x",
-                    rcount, inetFmt(croute->originAddr, s1), inetFmt(croute->group, s2),
-                    croute->ageValue,(croute->originAddr>0?"A":"I"),
-                    croute->vifBits);
-                  
-                croute = croute->nextroute; 
-        
+            RB_FOREACH(croute, rtabletree, &routing_table) {
+		log(LOG_DEBUG, 0, "#%d: Dst: %s, Age:%d, St: %s, OutVifs: 0x%08x",
+		    rcount, inetFmt(croute->group, s2),
+		    croute->ageValue,(TAILQ_EMPTY(&croute->originList)?"I":"A"),
+		    croute->vifBits);
+		{
+		    struct Origin *o;
+		    TAILQ_FOREACH(o, &croute->originList, next) {
+			log(LOG_DEBUG, 0, "#%d: Origin: %s floodIf %d pktcnt %d",
+			    rcount, inetFmt(o->originAddr, s1), o->flood, o->pktcnt);
+		    }
+		}
                 rcount++;
-            } while ( croute != NULL );
+	    }
         }
     
         log(LOG_DEBUG, 0, "\n-----------------------------------------------------\n");
